
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       compass_param.c
  * @author     baiyang
  * @date       2021-11-26
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_compass.h"
#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_compass_assign_param()
{
    sensor_compass *compass = sensor_compass_get_singleton();

    param_link_variable(PARAM_ID(COMPASS, COMPASS_USE), &compass->_use_for_yaw[0]);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_OFS_X), &compass->_state[0].offset.x);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_OFS_Y), &compass->_state[0].offset.y);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_OFS_Z), &compass->_state[0].offset.z);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DEC), &compass->_declination);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_LEARN), &compass->_learn);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_AUTODEC), &compass->_auto_declination);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_MOTCT), &compass->_motor_comp_type);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_MOT_X), &compass->_state[0].motor_compensation.x);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_MOT_Y), &compass->_state[0].motor_compensation.y);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_MOT_Z), &compass->_state[0].motor_compensation.z);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_ORIENT), &compass->_state[0].orientation);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_EXTERNAL), &compass->_state[0].external);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_USE2), &compass->_use_for_yaw[1]);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_OFS2_X), &compass->_state[1].offset.x);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_OFS2_Y), &compass->_state[1].offset.y);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_OFS2_Z), &compass->_state[1].offset.z);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_MOT2_X), &compass->_state[1].motor_compensation.x);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_MOT2_Y), &compass->_state[1].motor_compensation.y);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_MOT2_Z), &compass->_state[1].motor_compensation.z);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_ORIENT2), &compass->_state[1].orientation);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_EXTERN2), &compass->_state[1].external);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_USE3), &compass->_use_for_yaw[2]);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_OFS3_X), &compass->_state[2].offset.x);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_OFS3_Y), &compass->_state[2].offset.y);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_OFS3_Z), &compass->_state[2].offset.z);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_MOT3_X), &compass->_state[2].motor_compensation.x);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_MOT3_Y), &compass->_state[2].motor_compensation.y);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_MOT3_Z), &compass->_state[2].motor_compensation.z);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_ORIENT3), &compass->_state[2].orientation);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_EXTERN3), &compass->_state[2].external);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_DEV_ID), &compass->_state[0].dev_id);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DEV_ID2), &compass->_state[1].dev_id);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DEV_ID3), &compass->_state[2].dev_id);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_DEV_ID4), &compass->extra_dev_id[0]);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DEV_ID5), &compass->extra_dev_id[1]);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DEV_ID6), &compass->extra_dev_id[2]);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DEV_ID7), &compass->extra_dev_id[3]);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DEV_ID8), &compass->extra_dev_id[4]);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_DIA_X), &compass->_state[0].diagonals.x);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DIA_Y), &compass->_state[0].diagonals.y);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DIA_Z), &compass->_state[0].diagonals.z);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_ODI_X), &compass->_state[0].offdiagonals.x);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_ODI_Y), &compass->_state[0].offdiagonals.y);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_ODI_Z), &compass->_state[0].offdiagonals.z);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_DIA2_X), &compass->_state[1].diagonals.x);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DIA2_Y), &compass->_state[1].diagonals.y);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DIA2_Z), &compass->_state[1].diagonals.z);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_ODI2_X), &compass->_state[1].offdiagonals.x);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_ODI2_Y), &compass->_state[1].offdiagonals.y);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_ODI2_Z), &compass->_state[1].offdiagonals.z);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_DIA3_X), &compass->_state[2].diagonals.x);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DIA3_Y), &compass->_state[2].diagonals.y);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_DIA3_Z), &compass->_state[2].diagonals.z);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_ODI3_X), &compass->_state[2].offdiagonals.x);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_ODI3_Y), &compass->_state[2].offdiagonals.y);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_ODI3_Z), &compass->_state[2].offdiagonals.z);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_CAL_FIT), &compass->_calibration_threshold);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_OFFS_MAX), &compass->_offset_max);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_TYPEMASK), &compass->_driver_type_mask);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_FLTR_RNG), &compass->_filter_range);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_AUTO_ROT), &compass->_rotate_auto);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_PRIO1_ID), &compass->_priority_did_stored_list[0]);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_PRIO2_ID), &compass->_priority_did_stored_list[1]);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_PRIO3_ID), &compass->_priority_did_stored_list[2]);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_ENABLE), &compass->_enabled);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_SCALE), &compass->_state[0].scale_factor);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_SCALE2), &compass->_state[1].scale_factor);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_SCALE3), &compass->_state[2].scale_factor);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_OPTIONS), &compass->_options);

    param_link_variable(PARAM_ID(COMPASS, COMPASS_CUS_ROLL), &compass->_custom_roll);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_CUS_PIT), &compass->_custom_pitch);
    param_link_variable(PARAM_ID(COMPASS, COMPASS_CUS_YAW), &compass->_custom_yaw);
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_compass_priority_did_stored_save_param(uint8_t index, int32_t val)
{
    sensor_compass *compass = sensor_compass_get_singleton();

    if (index == 0) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_PRIO1_ID), val);
    } else if (index == 1) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_PRIO2_ID), val);
    } else if (index == 2) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_PRIO3_ID), val);
    }
}

/**
  * @brief       
  * @param[in]   index  
  * @param[in]   val  
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_compass_extra_dev_id_save_param(uint8_t index, int32_t val)
{
    sensor_compass *compass = sensor_compass_get_singleton();

    if (index == 0) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DEV_ID4), val);
    } else if (index == 1) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DEV_ID5), val);
    } else if (index == 2) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DEV_ID6), val);
    } else if (index == 3) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DEV_ID7), val);
    } else if (index == 4) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DEV_ID8), val);
    }
}

/**
  * @brief       
  * @param[in]   index  
  * @param[in]   
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_compass_extra_dev_id_save_param2(uint8_t index)
{
    sensor_compass *compass = sensor_compass_get_singleton();

    if (index == 0) {
        param_save_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID4));
    } else if (index == 1) {
        param_save_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID5));
    } else if (index == 2) {
        param_save_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID6));
    } else if (index == 3) {
        param_save_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID7));
    } else if (index == 4) {
        param_save_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID8));
    }
}

/**
  * @brief       
  * @param[in]   index  
  * @param[in]   val  
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_compass_extra_dev_id_set_param(uint8_t index, int32_t val)
{
    sensor_compass *compass = sensor_compass_get_singleton();

    if (index == 0) {
        param_set_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID4), val);
    } else if (index == 1) {
        param_set_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID5), val);
    } else if (index == 2) {
        param_set_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID6), val);
    } else if (index == 3) {
        param_set_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID7), val);
    } else if (index == 4) {
        param_set_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID8), val);
    }
}

/**
  * @brief       
  * @param[in]   index  
  * @param[in]   
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_compass_dev_id_set(uint8_t index, int32_t val)
{
    sensor_compass *compass = sensor_compass_get_singleton();

    if (index == 0) {
        param_set_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID), val);
    } else if (index == 1) {
        param_set_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID2), val);
    } else if (index == 2) {
        param_set_obj(PARAM_ID(COMPASS, COMPASS_DEV_ID3), val);
    }
}

/**
  * @brief       
  * @param[in]   index  
  * @param[in]   
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_compass_dev_id_set_and_save(uint8_t index, int32_t val)
{
    sensor_compass *compass = sensor_compass_get_singleton();

    if (index == 0) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DEV_ID), val);
    } else if (index == 1) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DEV_ID2), val);
    } else if (index == 2) {
        param_set_and_save(PARAM_ID(COMPASS, COMPASS_DEV_ID3), val);
    }
}

/**
  * @brief       
  * @param[in]   index  
  * @param[in]   
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_compass_offset_save(uint8_t index)
{
    sensor_compass *compass = sensor_compass_get_singleton();

    if (index == 0) {
        param_save_obj(PARAM_ID(COMPASS, COMPASS_OFS_X));
        param_save_obj(PARAM_ID(COMPASS, COMPASS_OFS_Y));
        param_save_obj(PARAM_ID(COMPASS, COMPASS_OFS_Z));
    } else if (index == 1) {
        param_save_obj(PARAM_ID(COMPASS, COMPASS_OFS2_X));
        param_save_obj(PARAM_ID(COMPASS, COMPASS_OFS2_Y));
        param_save_obj(PARAM_ID(COMPASS, COMPASS_OFS2_Z));
    } else if (index == 2) {
        param_save_obj(PARAM_ID(COMPASS, COMPASS_OFS3_X));
        param_save_obj(PARAM_ID(COMPASS, COMPASS_OFS3_Y));
        param_save_obj(PARAM_ID(COMPASS, COMPASS_OFS3_Z));
    }
}

// We only copy persistent params
void sensor_compass_mag_state_copy_from(mag_state* state_des, uint8_t index, const mag_state* state_src)
{
    if (index == 0) {
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_EXTERNAL), state_src->external);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ORIENT), state_src->orientation);

        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_OFS_X), state_src->offset.x);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_OFS_Y), state_src->offset.y);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_OFS_Z), state_src->offset.z);

        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_DIA_X), state_src->diagonals.x);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_DIA_Y), state_src->diagonals.y);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_DIA_Z), state_src->diagonals.z);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ODI_X), state_src->offdiagonals.x);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ODI_Y), state_src->offdiagonals.y);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ODI_Z), state_src->offdiagonals.z);

        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_SCALE), state_src->scale_factor);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_DEV_ID), state_src->dev_id);

        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_MOT_X), state_src->motor_compensation.x);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_MOT_Y), state_src->motor_compensation.y);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_MOT_Z), state_src->motor_compensation.z);
    } else if (index == 1) {
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_EXTERN2), state_src->external);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ORIENT2), state_src->orientation);

        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_OFS2_X), state_src->offset.x);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_OFS2_Y), state_src->offset.y);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_OFS2_Z), state_src->offset.z);

        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_DIA2_X), state_src->diagonals.x);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_DIA2_Y), state_src->diagonals.y);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_DIA2_Z), state_src->diagonals.z);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ODI2_X), state_src->offdiagonals.x);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ODI2_Y), state_src->offdiagonals.y);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ODI2_Z), state_src->offdiagonals.z);

        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_SCALE2), state_src->scale_factor);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_DEV_ID2), state_src->dev_id);

        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_MOT2_X), state_src->motor_compensation.x);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_MOT2_Y), state_src->motor_compensation.y);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_MOT2_Z), state_src->motor_compensation.z);
    } else if (index == 2) {
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_EXTERN3), state_src->external);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ORIENT3), state_src->orientation);

        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_OFS3_X), state_src->offset.x);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_OFS3_Y), state_src->offset.y);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_OFS3_Z), state_src->offset.z);

        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_DIA3_X), state_src->diagonals.x);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_DIA3_Y), state_src->diagonals.y);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_DIA3_Z), state_src->diagonals.z);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ODI3_X), state_src->offdiagonals.x);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ODI3_Y), state_src->offdiagonals.y);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_ODI3_Z), state_src->offdiagonals.z);

        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_SCALE3), state_src->scale_factor);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_DEV_ID3), state_src->dev_id);

        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_MOT3_X), state_src->motor_compensation.x);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_MOT3_Y), state_src->motor_compensation.y);
        param_set_and_save_ifchanged(PARAM_ID(COMPASS, COMPASS_MOT3_Z), state_src->motor_compensation.z);
    }

    state_des->expected_dev_id = state_src->expected_dev_id;
    state_des->detected_dev_id = state_src->detected_dev_id;
}

/*------------------------------------test------------------------------------*/


